//
// Created by xuan on 24-1-18.
//

#ifndef PROJECT_UPRE_DIJKSTRA_H
#define PROJECT_UPRE_DIJKSTRA_H

#include "UPRE_GLOBAL_DEFINE.h"

#define V 36
#define Ball_Num_Line 2
#define Ball_Num_Column 6

typedef struct Knots_state {
    float wx;
    float wy;
    float yaw;
    point BesselControlPoint_1;
    point BesselControlPoint_2;
    int statspeed;
    int maxspeed;
    int endspeed;
    uint8_t acceleratePercent;
    uint8_t descentPercent;
    uint8_t yawAdjustPercent;
} Knots_state;

extern uint8_t ball[Ball_Num_Line][Ball_Num_Column];
extern int PathKnots[V];
extern bool Flag_OnceCalculateDijkstra;

const extern Knots_state Actual_point_Bule[V];

const extern Knots_state Actual_point_Red[V];

extern int Dijkstra_CountKnots;

bool DIJKSTRA_CheckBall(void);

bool Dijkstra_PathPlanning(int CurrentKnots);

void Dijkstra_DijkstraCalculate(void);

bool Dijkstra_FirstPathPlanning(int CurrentKnots);

bool Dijkstra_TwoZonePathPlanning(int NextKnots);

unsigned char ManualToAutoMation_FindNearNode(Pose courent_pose);

static inline Knots_state Dijkstra_NodeMappingCoordinates_Bule(int knots) {
    return (Actual_point_Bule[knots]);
}

static inline Knots_state Dijkstra_NodeMappingCoordinates_Red(int knots) {
    return (Actual_point_Red[knots]);
}

#endif //PROJECT_UPRE_DIJKSTRA_H
